#include <math.h>
#define NRANSI
#include "nrutil.h"
#include <stdio.h>

#define MAXITS 200
#define EPS 1.0e-8
#define TOLF 1.0e-6
#define TOLX EPS
#define STPMX 100.0
#define TOLMIN 1.0e-6
// Mark's values:
/*#define MAXITS 200
#define EPS 1.0e-9
#define TOLF 1.0e-7
#define TOLX EPS
#define STPMX 100.0
#define TOLMIN 1.0e-7
*/
// originals:
/*#define MAXITS 200
#define EPS 1.0e-8
#define TOLF 1.0e-5
#define TOLX EPS
#define STPMX 100.0
#define TOLMIN 1.0e-7
*/
#define FREERETURN {free_dvector(fvec,1,n);free_dvector(xold,1,n);\
	free_dvector(w,1,n);free_dvector(t,1,n);free_dvector(s,1,n);\
	free_dmatrix(r,1,n,1,n);free_dmatrix(qt,1,n,1,n);free_dvector(p,1,n);\
	free_dvector(g,1,n);free_dvector(fvcold,1,n);free_dvector(d,1,n);\
	free_dvector(c,1,n);return;}

int nn;
double *fvec;
void (*nrfuncv)(int n, double v[], double f[]);

void broydn(double x[], int n, int *check,
	void (*vecfunc)(int, double [], double []))
{
	void fdjac(int n, double x[], double fvec[], double **df,
		void (*vecfunc)(int, double [], double []));
	double fminsq(double x[]);
	void lnsrch(int n, double xold[], double fold, double g[], double p[], double x[],
		 double *f, double stpmax, int *check, double (*func)(double []));
	void qrdcmp(double **a, int n, double *c, double *d, int *sing);
	void qrupdt(double **r, double **qt, int n, double u[], double v[]);
	void rsolv(double **a, int n, double d[], double b[]);
	int i,j,its,k,restrt,sing,skip;
	double den,f,fold,stpmax,sum,temp,test,*c,*d,*fvcold;
	double *g,*p,**qt,*s,**r,*t,*w,*xold;
	
	int numrestrts; //!!
	int ii,jj;
	double mtolf, mtolmin, mtolx;
	extern double jaceps; //!!
	extern int reuse_jacobian;
    extern int usedcar_in_jacobian;
	extern double sumfoc;
    extern int verbose;

	//extern double rsave[1500][1500];
	
	if (sumfoc < 0.1) {
		mtolf = TOLF / 50.0;
		mtolmin = TOLMIN / 50.0;
		mtolx = TOLX / 50.0;
	}
	else {
		mtolf = TOLF / 1.0;
		mtolmin = TOLMIN / 1.0;
		mtolx = TOLX / 1.0;
	}


	c=dvector(1,n);
	d=dvector(1,n);
	fvcold=dvector(1,n);
	g=dvector(1,n);
	p=dvector(1,n);
	qt=dmatrix(1,n,1,n);
	r=dmatrix(1,n,1,n);
	s=dvector(1,n);
	t=dvector(1,n);
	w=dvector(1,n);
	xold=dvector(1,n);
	fvec=dvector(1,n);
	nn=n;
	nrfuncv=vecfunc;
	f=fminsq(x);
	test=0.0;
	its=0;
	for (i=1;i<=n;i++)
		if (fabs(fvec[i]) > test)test=fabs(fvec[i]);
	if (test < 0.01*mtolf) {
		*check=0;
		FREERETURN
	}
	for (sum=0.0,i=1;i<=n;i++) sum += DSQR(x[i]);
	stpmax=STPMX*DMAX(sqrt(sum),(double)n);
	restrt=1;
	numrestrts=1; //!!
	for (its=1;its<=MAXITS;its++) {
		if (restrt) {
			//!!
			if (reuse_jacobian == 0 || its > 1) {
                usedcar_in_jacobian = 1;
				fdjac(n,x,fvec,r,vecfunc);
                usedcar_in_jacobian = 0;
				//for (ii=1;ii<=n;ii++) for (jj=1;jj<=n;jj++) rsave[ii][jj] = r[ii][jj];
			}
			else {
				(*vecfunc)(n,x,fvec); //one eval to make sure globals are up to date
				//printf("\n reusing jacobian\n");
				//for (ii=1;ii<=n;ii++) for (jj=1;jj<=n;jj++) r[ii][jj] = rsave[ii][jj];
				//printf("\nr11 %lf  rnn %lf", r[1][1], r[n][n]);
			}

			qrdcmp(r,n,c,d,&sing);
			if (sing) nrerror("singular Jacobian in broydn");
			for (i=1;i<=n;i++) {
				for (j=1;j<=n;j++) qt[i][j]=0.0;
				qt[i][i]=1.0;
			}
			for (k=1;k<n;k++) {
				if (c[k]) {
					for (j=1;j<=n;j++) {
						sum=0.0;
						for (i=k;i<=n;i++)
							sum += r[i][k]*qt[i][j];
						sum /= c[k];
						for (i=k;i<=n;i++)
							qt[i][j] -= sum*r[i][k];
					}
				}
			}
			for (i=1;i<=n;i++) {
				r[i][i]=d[i];
				for (j=1;j<i;j++) r[i][j]=0.0;
			}
		} else {
			for (i=1;i<=n;i++) s[i]=x[i]-xold[i];
			for (i=1;i<=n;i++) {
				for (sum=0.0,j=i;j<=n;j++) sum += r[i][j]*s[j];
				t[i]=sum;
			}
			skip=1;
			for (i=1;i<=n;i++) {
				for (sum=0.0,j=1;j<=n;j++) sum += qt[j][i]*t[j];
				w[i]=fvec[i]-fvcold[i]-sum;
				if (fabs(w[i]) >= EPS*(fabs(fvec[i])+fabs(fvcold[i]))) skip=0;
				else w[i]=0.0;
			}
			if (!skip) {
				for (i=1;i<=n;i++) {
					for (sum=0.0,j=1;j<=n;j++) sum += qt[i][j]*w[j];
					t[i]=sum;
				}
				for (den=0.0,i=1;i<=n;i++) den += DSQR(s[i]);
				for (i=1;i<=n;i++) s[i] /= den;
				qrupdt(r,qt,n,t,s);
				for (i=1;i<=n;i++) {
					if (r[i][i] == 0.0) nrerror("r singular in broydn");
					d[i]=r[i][i];
				}
			}
		}
		for (i=1;i<=n;i++) {
			for (sum=0.0,j=1;j<=n;j++) sum += qt[i][j]*fvec[j];
			g[i]=sum;
		}
		for (i=n;i>=1;i--) {
			for (sum=0.0,j=1;j<=i;j++) sum += r[j][i]*g[j];
			g[i]=sum;
		}
		for (i=1;i<=n;i++) {
			xold[i]=x[i];
			fvcold[i]=fvec[i];
		}
		fold=f;
		for (i=1;i<=n;i++) {
			for (sum=0.0,j=1;j<=n;j++) sum += qt[i][j]*fvec[j];
			p[i] = -sum;
		}
		rsolv(r,n,d,p);
		lnsrch(n,xold,fold,g,p,x,&f,stpmax,check,fminsq);
		test=0.0;
		for (i=1;i<=n;i++)
			if (fabs(fvec[i]) > test) test=fabs(fvec[i]);
		if (test < mtolf) {
			*check=0;
			FREERETURN
		}
		if (*check) {
			if (restrt) {   //!!
				if (numrestrts > 6) FREERETURN 
				else {
					jaceps = jaceps / (numrestrts + 1);  //!! scale down jaceps and restart again
					if (verbose) printf("\n restart %d BROYDN internal jaceps reset to  %f", numrestrts+2, jaceps);
					numrestrts++;
				}
				
			} else {
				test=0.0;
				den=DMAX(f,0.5*n);
				for (i=1;i<=n;i++) {
					temp=fabs(g[i])*DMAX(fabs(x[i]),1.0)/den;
					if (temp > test) test=temp;
				}
				if (test < mtolmin)  FREERETURN
				else restrt=1;
				numrestrts=1;
			}
		} else {
			restrt=0;
			test=0.0;
			for (i=1;i<=n;i++) {
				temp=(fabs(x[i]-xold[i]))/DMAX(fabs(x[i]),1.0);
				if (temp > test) test=temp;
			}
			if (test < mtolx)  FREERETURN
		}
	}
	if (verbose) printf ("maxits exceeded in broydn, continue anyway"); //nrerror("MAXITS exceeded in broydn");
	FREERETURN
}
#undef MAXITS
#undef EPS
#undef TOLF
#undef TOLMIN
#undef TOLX
#undef STPMX
#undef FREERETURN
#undef NRANSI
